#include <stdio.h>
#include "proto.h"
int LED = 7; // LED connected to pin 6
void setup() {
  // put your setup code here, to run once:
   pinMode (LED , OUTPUT);
   Serial.begin(115200);

}
typedef struct {
  uint8_t fun;
  uint8_t len;
  uint8_t data[253];
}Command;


Command gCommand;
bool gInProgress = false;
uint8_t gReceived = 0;
uint8_t gReserve[253];

uint8_t lookup[100] = {
  1000,500,800,700,
};
void AxisMove(uint8_t axis,uint16_t cnt){
   for(uint16_t i = 0;i < cnt; i++){
     digitalWrite(axis,HIGH); // turn on led
     delayMicroseconds(100); // delay for 500 ms
     digitalWrite(axis,LOW); // turn off led
     delayMicroseconds(100); // delay for 500 ms    
   }
}



void SortPackage(uint8_t* buf,uint8_t len){
      // 是否有未完成的组包
      if(!gInProgress){
          //刚好组成一包数据
        if(buf[1] == len){
          //校验
          uint8_t verify = 0;
          for(uint8_t i = 0;i < len - 1;i++){
            verify += buf[i];
          }
          if(verify == buf[len - 1]){
              if(len == 3){  //证明没有包数据
                CallBackOnPackage(buf[0],nullptr,0);
              }else{
                CallBackOnPackage(buf[0],buf + 2,len - 3);
              }
            Serial.print("read a pakcage");
          }else{
              Serial.print("verify error");
          }
        }
        // 至少有一个包
        if(buf[1] < len){
          //先获取第一个包
          uint8_t firstlen = buf[1];
          uint8_t verify = 0;
          for(uint8_t i = 0;i < firstlen - 1;i++){
            verify += buf[i];
          }
          if(verify == buf[firstlen - 1]){
            if(firstlen == 3){  //证明没有包数据
                CallBackOnPackage(buf[0],nullptr,0);
              }else{
                CallBackOnPackage(buf[0],buf + 2,firstlen - 3);
              }
            Serial.print("read a pakcage\r\n");
          }else{
              Serial.print("verify error");
          }
          SortPackage(buf + firstlen,len -  firstlen);
        }
      }else{
        
      } 
}


void GetCommand(){
  uint8_t buf[256];
  uint8_t len = 0;
  if(Serial.available() > 2){
    len =  Serial.available();
    Serial.print("len ");
    Serial.print(len,HEX);
    Serial.print("\r\n");

    for(uint8_t i = 0;i < len;i++){
      buf[i] = Serial.read();
      Serial.print("0x");
      Serial.print(buf[i],HEX);
      Serial.print(" ");
    }
    Serial.print("\r\n");
    SortPackage(buf,len);

  }
}
void loop() {
  GetCommand();
  delay(200);

  // put your main code here, to run repeatedly:
}
